//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <bsp.h>
#include <mmu.h>
#include <init.h>
#include <bootinfo.h>
#include "flash.h"

#define CurUartPhy              BTUartBasePhy
#define FFUartBasePhy  0x40100000
#define BTUartBasePhy  0x40200000

#define UartRBR(_Base_)  ( (_Base_) + 0x00 )  /* Receive Buffer Register (read only) */
#define UartTHR(_Base_)  ( (_Base_) + 0x00 )  /* Transmit Holding Register (write only) */
#define UartIER(_Base_)  ( (_Base_) + 0x04 )  /* Interrupt Enable Register (read/write) */
#define UartIIR(_Base_)  ( (_Base_) + 0x08 )  /* Interrupt ID Register (read only) */
#define UartFCR(_Base_)  ( (_Base_) + 0x08 )  /* FIFO Control Register (write only) */
#define UartLCR(_Base_)  ( (_Base_) + 0x0C )  /* Line Control Register (read/write) */
#define UartMCR(_Base_)  ( (_Base_) + 0x10 )  /* Modem Control Register (read/write) */
#define UartLSR(_Base_)  ( (_Base_) + 0x14 )  /* Line Status Register (read only) */
#define UartMSR(_Base_)  ( (_Base_) + 0x18 )  /* Modem Status Register (read only) */
#define UartSPR(_Base_)  ( (_Base_) + 0x1C )  /* Scratch Pad Register (read/write) */
#define UartISR(_Base_)  ( (_Base_) + 0x20 )  /* Infrared Selection Register (read/write) */
#define UartDLL(_Base_)  ( (_Base_) + 0x00 )  /* Divisor Latch Low Register (DLAB = 1) (read/write) */
#define UartDLH(_Base_)  ( (_Base_) + 0x04 )  /* Divisor Latch High Register (DLAB = 1) (read/write) */

static void UARTSendChar(unsigned long UartBase, unsigned char c)
{
    //UnusedParameter(UartBase);
    if (UartBase == 0)
        UartBase = CurUartPhy;
    else
        UartBase = FFUartBasePhy;

    /* wait for room in the tx FIFO */
    while( (Inl(UartLSR(UartBase)) & (0x1 << 6)) == 0) ;

    Outl(UartRBR(UartBase), c);

    return;
}

static void WriteByte(unsigned char c)
{
    UARTSendChar(0,c);
}

//static void WriteNumber(unsigned long uVal)
//{
//    unsigned char * p = (unsigned char*)&uVal;
//    p += 3;
//    WriteByte('0');
//    WriteByte('X');
//    for (int i = 0; i < 8; i++) {
//        unsigned char c = *p;
//        c &= 0x0f0;
//        c >>= 4;
//        c &= 0x0f;
//        if (c > 9)
//            c += 0x37;
//        else
//            c |= 0x30;
//        WriteByte(c);
//        uVal <<= 4;
//        }
//    WriteByte('\n');
//}

EXTERN_C DECL_CODEINIT void CDECL WriteFlash(BootInfo *pBootInfo)
{
//    kprintf("<Boot Info> (0x%x)\n"
//            "LoadAddress:       0x%x\n"
//            "ImageBase:         0x%x\n"
//            "ImageSize:         0x%x\n"
//            "MemorySize:        0x%x\n"
//            "WriteMagic:        0x%x\n",
//            pBootInfo,
//            pBootInfo->LoadAddress,
//            pBootInfo->ImageBase,
//            pBootInfo->ImageSize,
//            pBootInfo->MemorySize,
//            pBootInfo->WriteMagic);

    if (WRITE_MAGIC != pBootInfo->WriteMagic) return;

    virtaddr_t vaFlashBase = HwFlashBase; //;DzKIoRemap(HwFlashBase, 0x10000000/*32 * 1024 * 1024*/);

    unsigned long Image = pBootInfo->LoadAddress;
    unsigned long ImageSize = RoundUp(pBootInfo->ImageSize, 32);
    unsigned long FlashSectSize = HwFlashSectorSize;
    unsigned long FlashOffset = HwFlashOffset;

//    kprintf("Write image 0x%08x, size 0x%08x to flash (0x%08x ~ 0x%08x):\n",
//            Image, ImageSize, FlashOffset,
//            RoundUp((FlashOffset+ImageSize), FlashSectSize));
//    WriteNumber(Image);
//    WriteNumber(ImageSize);
//    WriteNumber(FlashOffset);
//    WriteNumber(RoundUp((FlashOffset+ImageSize), FlashSectSize));
    WriteByte('>');

    for (;; FlashOffset += FlashSectSize) {
//        kprintf("Program Flash 0x%08x ...\n", FlashOffset);
//        WriteNumber(FlashOffset);
        FlashEraseSector((ulong_t)vaFlashBase, FlashOffset);

        unsigned long WriteSize =
                (ImageSize > FlashSectSize)? FlashSectSize : ImageSize;

        FlashWriteBuffer(
            (ulong_t)vaFlashBase, FlashOffset, (unsigned char*)Image, WriteSize);

        if (0 == memcmp(
            (void*)((ulong_t)vaFlashBase + FlashOffset), (void*)Image, WriteSize)) {
//            kprintf("Verify OK\n");
            WriteByte('@');
        }
        else {
//            kprintf("Verify ERR\n");
//            assert(0);
            WriteByte('e');
        }
        Image += WriteSize;
        ImageSize -= WriteSize;

        if (ImageSize == 0) break;
    }

//    kprintf("\nWrite image OK!\n");
}
